/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#ifndef _JOYSTICK_H_
#define _JOYSTICK_H_

// ================================================================== includes

// ModuleCore include should be above ROS and OceanSHELL includes.
#include <osl_core/ModuleCore.h>
#include <osl_core/TimerMillis.h>

#include <geometry_msgs/WrenchStamped.h>
#include <auv_msgs/BodyVelocityReq.h>
#include <auv_msgs/NavSts.h>
#include <auv_msgs/WorldWaypointReq.h>

// ========================================================== external defines

/*
 * BUTTON MAPPING INFORMATION
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~
 * Buttons appear to be indexed from 0, but otherwise keep to the same order
 * as written on the joypad itself.  So button 1 is pad.bPos[0], etc.
 */

#define MAX_AXIS 16
#define MAX_BUTTON 16
#define DEFAULT_DEVICE_PATH "/dev/input/js0"

typedef struct {
	unsigned char axisCount;
	unsigned char buttonCount;
	int fd;
	int version;
	char devName[80];
	int aPos[MAX_AXIS];
	int bPos[MAX_BUTTON];
	bool changed;
	js_event ev;
} pad_data_t;

// ===================================================================== class
class Joystick : public osl_core::ModuleCore
{
private:
	bool init(int argc, char * argv[]);

	void doWork();

	void cleanup();

	float scaleStick(int value, float limit);

	bool readLatestState();

	void printPadState();

	void sendForceMsg(float x, float y, float z, float roll, float pitch, float yaw);

	void sendVelocityMsg(float x, float y, float z, float roll, float pitch, float yaw);

	void setWaypoint();

	void unsetWaypoint();

	void sendWaypoint();

	void processNavSts(const auv_msgs::NavStsConstPtr & msg);

	ros::Publisher m_rosVehicleForcePub;
	ros::Publisher m_rosVehicleVelPub;
	ros::Publisher m_rosWorldWaypointPub;
	ros::Subscriber m_rosNavStsSub;


	bool m_haveNav;
	bool m_waypointActive;
	auv_msgs::NavStsConstPtr m_navMsg;
	auv_msgs::WorldWaypointReq m_msgPubWorld;
	bool directMode;
	bool invertPitch;
	bool forceSend;
	bool overrideForceMode;
	bool velocityMode;
	std::string deviceName;

	osl_core::TimerMillis sendTimer;
	int sendPeriod;

	pad_data_t pad;

	bool lastValuesNonZero;
	double m_idWaypoint;
};

#endif
